#include "arduino.h"
#include "SerialServoFunctions.h" 
void PositionFineAdjustment(HardwareSerial &SerialX, uint8_t id, int adjustment)
{
  byte buf[9];
  buf[0] = buf[1] = HTE_ROBOT_MESSAGE_HEAD;
  buf[2] = id;
  buf[3] = 5;
  buf[4] = 3;
  buf[5] = 0x14;
  buf[6] = GET_HIGH_BYTE(adjustment);
  buf[7] = GET_LOW_BYTE(adjustment);
  buf[8] = CheckSum(buf);
  SerialX.write(buf, 9);
}

byte CheckSum(byte buf[])
{
  byte i;
  uint16_t temp = 0;
  for (i = 2; i <= buf[3]+2; i++) { //数据长度(ID号+指令类型+参数所有字节数)+数据长度本身的一个字节，又因为是角标访问，所以是buf[3]+2
    temp += buf[i];
  }
  temp = ~temp;
  i = (byte)(temp & 255);
  return i;
}

void SerialServoUnload(HardwareSerial &SerialX, uint8_t id)
{
  byte buf[8];
  buf[0] = buf[1] = HTE_ROBOT_MESSAGE_HEAD;
  buf[2] = id;
  buf[3] = 4;
  buf[4] = 3;
  buf[5] = 0x28;
  buf[6] = 0;
  buf[7] = CheckSum(buf);
  
  SerialX.write(buf, 8);
}

void SerialServoload(HardwareSerial &SerialX, uint8_t id)
{
  byte buf[8];
  buf[0] = buf[1] = HTE_ROBOT_MESSAGE_HEAD;
  buf[2] = id;
  buf[3] = 4;
  buf[4] = 3;
  buf[5] = 0x28;
  buf[6] = 1;
  buf[7] = CheckSum(buf);
  
  SerialX.write(buf, 8);

}

void ServoRGBSet(HardwareSerial &SerialX, uint8_t id, uint8_t R, uint8_t G, uint8_t B)
{
  byte buf[11];
  buf[0] = buf[1] = 0xff;                        //255
  buf[2] = id;                                  //254
  buf[3] = 7;                                   //7
  buf[4] = 3;                                   //3
  buf[5] = 0x43;                                  //2A  42
  buf[6] = R;                                   //红灯值
  buf[7] = G;                                   //绿灯值
  buf[8] = B;                                   //蓝灯值
  buf[9] = 0;                                   //空位
  buf[10] = CheckSum(buf);
  SerialX.write(buf, 11);
}

void ServoMove(HardwareSerial &SerialX, uint8_t id, int16_t position, uint16_t time)
{
  byte buf[11];
  if(position < 0)
    position = 0;
  if(position > 4096)
    position = 4096;
  buf[0] = buf[1] = 255;                        //255
  buf[2] = id;                                  //254
  buf[3] = 7;                                   //7
  buf[4] = 3;                                   //3
  buf[5] = 42;                                  //2A  42
  buf[6] = GET_HIGH_BYTE(position);              //低八位整过来
  buf[7] = GET_LOW_BYTE(position);              //0    0
  buf[8] = GET_HIGH_BYTE(time);                  //0
  buf[9] = GET_LOW_BYTE(time);                 //3
  buf[10] = CheckSum(buf);
  SerialX.write(buf, 11);
}

void SetID(HardwareSerial &SerialX, uint8_t oldID, uint8_t newID)
{
  byte buf[7];
  buf[0] = buf[1] = 255;
  buf[2] = oldID;//if don't know the oldID, please set it to 254
  buf[3] = 4;
  buf[4] = 3;
  buf[5] = newID;
  buf[6] = CheckSum(buf);
  SerialX.write(buf, 7);
}

